#include "FieldBoardManager.h"

/* CONSTRUCTOR */
FieldBoardManager::FieldBoardManager(FieldBoardManagerData fieldBoardManagerData)
{
  for(int i=0; i<NB_OF_SWITCH_RAIL_DRIVERS; i++)
  {
    m_switchRailDrivers[i] = new SwitchRailDriver(*(fieldBoardManagerData.switchRailDriverData[i]));
  }
  m_communication = Communication::add(&Serial1, this);
  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);
}

/**
* \brief Analyse a command received over serial port
* \param command
* \return none
*/
void FieldBoardManager::commandReceived(Command* command)
{
  byte commandType = command->getType();
  byte commandDeviceId = command->getDeviceId();
  byte commandValue = command->getValue();
  int index;
  int randomval = rand() % 2;
  switch (commandType)
  {
    case CommandType::SWITCHRAIL_POSITION_UPDATE:
      index = getSwitchRailDriverIndexFromId(commandDeviceId);
      if (randomval == 0)
      {
        m_communication->sendCommand(new Command(CommandType::SWITCHRAIL_POSITION_UPDATE_ACK, commandDeviceId, CommandStatus::DELAYED));
        delay(5000);
      }
      if (index == -1)
      {
        /* error... */
      }
      else
      {
        if (commandValue == 0x00)
        {
          m_switchRailDrivers[index]->goStraightOn();
        }
        else
        {
          if (commandValue == 0xFF)
          {
            m_switchRailDrivers[index]->changeDirection();
          }
          else
          {
            /* Unknown Command Status */
          }
        }
      }
      m_communication->sendCommand(new Command(CommandType::SWITCHRAIL_POSITION_UPDATE_ACK, commandDeviceId, CommandStatus::ACCEPTED));
      break;
      
    case CommandType::TRAIN_SPEED_MOVE_FORWARD_UPDATE:
      analogWrite(8, 0);
      analogWrite(9, commandValue);
      break;
      
    case CommandType::TRAIN_SPEED_MOVE_BACKWARD_UPDATE:
      analogWrite(8, commandValue);
      analogWrite(9, 0);
      break;
      
    case CommandType::TRAIN_SPEED_STOP_UPDATE:
      analogWrite(8, 0);
      analogWrite(9, 0);
      break;
      
    default:
      break;
  }
}

/**
* \brief Get the switch rail driver index from Id
* \param Id
* \return index
*/
int FieldBoardManager::getSwitchRailDriverIndexFromId(byte id)
{
  int index = -1;                              // Initialize index
  for(int i=0; i<NB_OF_SWITCH_RAIL_DRIVERS; i++)
  {
    if(m_switchRailDrivers[i]->getId() == id)
    {
      index = i;
      break;
    }
    else {
      /* not the correct switchRail, continue */
    }
  }
  return index;
}
